/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef _TRAP_TRAJ_H
#define _TRAP_TRAJ_H

//#include "main.h"
#include <stdbool.h>
#include <math.h>

#define SQ(x) 					((x) * (x))
#define trapTrajDT							(0.001f)

#define TRAJ_getModelPoint(_Traj_)				((_Traj_).s)

/* 	
	获取模型位置已行驶的比例(0-1)
*/
#define TRAJ_ModelScale_ForDis(_Traj_)				(float)( ((_Traj_).s / (float)(_Traj_).temp.EndPoint) > 1 ? \
																									 1 : ((_Traj_).s / (float)(_Traj_).temp.EndPoint) < 0 ? \
																									 0 : ((_Traj_).s / (float)(_Traj_).temp.EndPoint) ) 
												

/* 获取是否结束*/
#define TRAJ_getStep(x)						(x.step)
																									 
/* 获取是否结束*/
#define TRAJ_getEndFlag(x)					(TRAJ_getStep(x) >= 6)

#define TRAJ_Control(ctrl,in,i)  	((in) > (ctrl) ? \
																	((ctrl) + (i) > (in) ? (in) : (ctrl) + (i)) : \
																	((ctrl) - (i) < (in) ? (in) : (ctrl) - (i)))
typedef struct {
	float velBase;
	float EndPoint;
	float accelTime;
	float speedTime;
	float decelTime;
	float velMax, accj, decj;
	float a1,a2,a3;
	float v1,v2,v3;
	float s1,s2,s3,s4;
}TrajTemp_t;

typedef struct {
	// Step
	float s;
	float v;
	float a;
	float j;
	
	float TotalTime;
	float t;
	unsigned char step;
	TrajTemp_t temp;
}Traj_Typedef;

void TRAJ_plan(Traj_Typedef *Traj,float dX, float velBase, float velMax, float accXin, float decXin);
void TRAJ_eval(Traj_Typedef *Traj, float dt);

#endif /* _TRAP_TRAJ_H */

/******************* (H) COPYRIGHT 2023 FUYEBING*****END OF FILE************/